
#include "pid.h"

// 限幅
static float PID_Limit(float value, float min, float max)
{
    if (value < min)
        return min;
    else if (value > max)
        return max;
    return value;
}

float pd_calc(pid_control_t *pid, float error)
{
    float out;
    float dItem;

    if (!pid)
        return 0;

    /* 误差变化量 */
    dItem = error - pid->last_error;

    if (pid->filter)
        dItem = pid->filter(dItem);

    /* 计算PD控制输出量 */
    out = pid->kp * error + pid->kd * dItem;
    /* 输出限幅 */
    out = PID_Limit(out, pid->output_min, pid->output_max);
    /* 记录当前误差 */
    pid->last_error = error;

    return out;
}

float pi_calc(pid_control_t *pid, float error)
{
    float out;

    if (!pid)
        return 0;

    /* 计算积分项 */
    pid->integral_sum += pid->ki * error;
    /* 积分项限幅 */
    pid->integral_sum = PID_Limit(pid->integral_sum, pid->integral_sum_min, pid->integral_sum_max);
    /* 计算PI控制输出量 */
    out = pid->kp * error + pid->integral_sum;
    /* 输出限幅 */
    out = PID_Limit(out, pid->output_min, pid->output_max);

    return out;
}

float pid_calc(pid_control_t *pid, float error)
{
    float out;
    float dItem;

    /* 计算积分项 */
    pid->integral_sum += pid->ki * error;
    /* 积分项限幅 */
    pid->integral_sum = PID_Limit(pid->integral_sum, pid->integral_sum_min, pid->integral_sum_max);
    /* 误差变化量 */
    dItem = error - pid->last_error;
    if (pid->filter)
        dItem = pid->filter(dItem);
    /* 计算PID控制输出量 */
    out = pid->kp * error + pid->integral_sum + pid->kd * dItem;
    /* 输出量限幅 */
    out = PID_Limit(out, pid->output_min, pid->output_max);
    /* 更新误差项 */
    pid->last_error = error;

    return out;
}
